#include "filter.h"
#include "main.h"

//IIR 2-Order Filter Fs=20kHz,Fc=1466Hz Butterworth
const float NUM_2[3] = {0.03963987,  0.07927974,  0.03963987};
const float DEN_2[3] = {1,   -1.36319565,    0.52175513};

// //IIR 3-Order Filter Fs=500Hz,Fc=20Hz Butterworth
const float NUM_3[4] = {0.001567010302, 0.004701030906, 0.004701030906, 0.001567010302};
const float DEN_3[4] = {1,   -2.498608351,    2.115254164,  -0.6041097045};

/**
  * @brief  2-order IIR filter
  * @param[in]  x    input in fp32
  * @param[in]  ch   input channel, there are MAX_FILTER_CH_2 channels available
  * @return output fp32
  */
float iir_filter_2(float x, unsigned int ch){
	static float y2,x2_n1[MAX_FILTER_CH_2],x2_n2[MAX_FILTER_CH_2]
					,y2_n1[MAX_FILTER_CH_2],y2_n2[MAX_FILTER_CH_2];
	y2 = NUM_2[0]*x + NUM_2[1]*x2_n1[ch] + NUM_2[2]*x2_n2[ch]
						- DEN_2[1]*y2_n1[ch] - DEN_2[2]*y2_n2[ch];
	//if(y<EPS && y>-EPS) y = 0.0;
	y2_n2[ch]=y2_n1[ch]; y2_n1[ch]=y2;
	x2_n2[ch]=x2_n1[ch]; x2_n1[ch]=x;
	return y2;
}

/**
  * @brief  3-order IIR filter
  * @param[in]  x    input in fp32
  * @param[in]  ch   input channel, there are MAX_FILTER_CH_2 channels available
  * @return output fp32
  */

float iir_filter_3(float x, unsigned int ch){
	static float y3,x3_n1[MAX_FILTER_CH_3],x3_n2[MAX_FILTER_CH_3],x3_n3[MAX_FILTER_CH_3]
					,y3_n1[MAX_FILTER_CH_3],y3_n2[MAX_FILTER_CH_3],y3_n3[MAX_FILTER_CH_3];
	
	y3 = NUM_3[0]*x + NUM_3[1]*x3_n1[ch] + NUM_3[2]*x3_n2[ch] + NUM_3[3]*x3_n3[ch]
						- DEN_3[1]*y3_n1[ch] - DEN_3[2]*y3_n2[ch] - DEN_3[3]*y3_n3[ch];
	
	//if(y<EPS && y>-EPS) y = 0.0;
	y3_n3[ch]=y3_n2[ch]; y3_n2[ch]=y3_n1[ch]; y3_n1[ch]=y3;
	x3_n3[ch]=x3_n2[ch]; x3_n2[ch]=x3_n1[ch]; x3_n1[ch]=x;
	return y3;
}
